Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fixed the orientation of covariance view to match with the interpreta… #1576

Open
wants to merge 1 commit into
base: melodic-devel
Choose a base branch
from

Conversation

ViktorWalter
Copy link

@ViktorWalter ViktorWalter commented Jan 15, 2021

Fixed the orientation part of covariance view to match with the interpretation of the covariance in e.g. geometry_msgs::PoseWithCovariance used by tf2.
This means, that the "tangent covariance" on the tips of the axis vector visualizations will actually show the orientation uncertainties/noises in the frame of the world, not in the frame of the object/body as was the case previously.

…tion of the covariance in e.g. geometry_msgs::PoseWithCovariance used by tf2.
@rhaschke
Copy link
Contributor

Could you please check the overlap of your PR with #1540.

@ViktorWalter
Copy link
Author

ViktorWalter commented Jan 15, 2021

Could you please check the overlap of your PR with #1540.

I checked now - I would say their solution is incorrect - they are "manually" fixing the issues they discovered by swapping axes.
I have observed their issues myself before. The cause of this issue was - as far as I can tell - the incorrect interpretation of the orientation covariance in general in cases where the orientation mean is not aligned with the world (frame in which we are visualizing). How exactly this leads to some of the "tangent covariances" appearing perpendicular to what one would expect them is difficult to interpret, but I'd say that it is because they are created from submatrices of covariance matrix in the wrong frame. If the orientation itself changes - e.g. we rotate the mean about x by 90 deg. - the covariance visualization should change to reflect this.
Edit: I see that their pose is actually aligned. Then I don't know what the issue is there - I have not observed this happening in such cases.

@ViktorWalter
Copy link
Author

Ok, I can replicate the issue in #1540 with both versions. This implies that this is a separate issue.

@ViktorWalter
Copy link
Author

ViktorWalter commented Jan 15, 2021

image
Here is a demo - the cyan visualizations are with the original implementation of the orientation covariance display, the purple are mine. On he left, they are displayed with the same mean as in #1540, on the right the orientation itself is different by 90 degrees, but the orientation covariance is unchanged. The left visualizations both exhibit the issue in #1540, but on the right you can see the effects of my fix. My assesment is that the author of #1540 was right - the 2D covariance on the "Z" (blue) axis is rotated by 90 degrees. This is separate from my issues.

@rhaschke
Copy link
Contributor

I'm afraid that I cannot judge your code change, because I don't exactly know the definition of the Pose covariance. Given 6d-vectors x = (p, w) and their counterparts y = (Rp, Rw) represented in another frame to compute the covariance from, the corresponding covariance matrices should be defined as: CX = <(x-µ) (x-µ)^t> and CY = <(Rx-Rµ) (Rx-Rµ)^t> = RCXR^t.
However, your change only applies the orientation change to the orientation part, while according to above equation it should be applied to both, position and orientation, shouldn't it?

@ViktorWalter
Copy link
Author

ViktorWalter commented Jan 18, 2021

No. The position covariance is already expressed in the frame in the message header. Otherwise, changing the orientation would rotate the position covariance too, which it doesn't. It Does change the orientation covariance, which it should not - e.g if we have large noise in yaw, but set the orientation to 90 deg in pitch, the visualization will imply that there is a large noise in roll, which is wrong. My change makes the orientation covariance consistent with the position covariance interpretation - by recalculating the covariance matrix, we can extract the "tangent covariances" (I don't know what to call them) such that they will represent the noise in the viewing frame. The reason that this change is applied only to the orientation covariance is that as opposed to the position covariance this visualization is affixed to the axis gizmo's full pose. Edit.: Obviously, an alternative solution would be to leave the covariance as is, and affix it to the frame axes instead of the visualized body axes.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants